#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

int main(int argc, char **argv)
{
	ros::init(argc, argv, "cartesian_path_demo", ros::init_options::AnonymousName);

	// 创建一个异步的自旋线程（spinning thread）
	ros::AsyncSpinner spinner(1);
	spinner.start();

	  //initialize visual tools
  	namespace rvt = rviz_visual_tools;
  	moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
	moveit::planning_interface::MoveGroupInterface move_group("arm");

	// 设置一个目标位置（路点）
	geometry_msgs::Pose target_pose;
	target_pose.orientation.w = 0;
	target_pose.orientation.x = 0;
	target_pose.orientation.y = 0;
	target_pose.orientation.z = 0;
	target_pose.position.x = 0.0;
	target_pose.position.y = 0.15;
	target_pose.position.z = 0.4;
	std::vector<geometry_msgs::Pose> waypoints;
	waypoints.push_back(target_pose);

	geometry_msgs::Pose target_pose1 = target_pose;

	target_pose1.position.z -= 0.2;
	waypoints.push_back(target_pose1);

	// target_pose1.position.y -= 0.1;
	// waypoints.push_back(target_pose1);

	// target_pose1.position.z += 0.2;
	// target_pose1.position.y += 0.1;
	// target_pose1.position.x -= 0.2;
	// waypoints.push_back(target_pose1);

	move_group.setMaxVelocityScalingFactor(0.1);

	// 笛卡尔空间下的路径规划
	moveit_msgs::RobotTrajectory trajectory;
	const double jump_threshold = 0.0;
	const double eef_step = 0.01;
	double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);

	ROS_INFO_NAMED("msg","planning is %s,      %.2f", fraction>0?"successful":"failed",fraction*100);
	// 生成机械臂的运动规划数据
	moveit::planning_interface::MoveGroupInterface::Plan plan;
	plan.trajectory_ = trajectory;

	visual_tools.deleteAllMarkers();
	visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
	for (std::size_t i = 0; i < waypoints.size(); ++i)
		visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
	visual_tools.trigger();
	visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

	// 执行运动
	move_group.execute(plan);

	ros::shutdown(); 
	return 0;
}
